#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <nmea_msgs/Gpgga.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <string>
#include <sstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <math.h>
using namespace std;

#define PI 3.141592653589793
int ggaflage=0,traflage=0,xyzflage=0,dlen=0,data_c=0,out=5;
double gnss_pitch=0,gnss_roll=0,gnss_heading=0,enu_x=0,enu_y=0,enu_z=0,enu_xv[6]={0},enu_yv[6]={0},enu_zv[6]={0},z_yaw=0,t_yaw=0,v_secs[2]={-1,-1},v_nano[2]={-1,-1},v_time=-1;
bool status_gps = true;
serial::Serial ser;
nmea_msgs::Gpgga gpgga_msg;
nav_msgs::Odometry msg_gnssodometry;
sensor_msgs::NavSatFix msg_navsatfix;
template <typename Type>
Type stringToNum(const string &str)  //字符串转数字
{
    istringstream iss(str);
    Type num;
    iss >> num;
    return num;
}

template <typename Type>
string numToString (const Type &num) //数字转字符串
{
        stringstream ss;
        string s;

        ss << num;
        s = ss.str();
        return s;
}
struct my_pose
{
    double latitude;
    double longitude;
    double altitude;
};
//将纬度和经度值从度数转换为弧度。
double rad(double d)
{
        return d * 3.1415926 / 180.0;
}
static double EARTH_RADIUS = 6378.137;//地球半径
bool init = false;
int init_status=0,enu_use=0;
string enu_s="ECEF";
my_pose init_pose;
double max_min_chioce(double value_tmp[],int number)
{
   double max_value, min_value;
   double sample_value = 0;
   double sum = 0;
   int i;

   max_value = 	value_tmp[0];
   min_value = 	value_tmp[0];
   for( i =0; i< number; i++)
    {
         if(value_tmp[i] >  max_value)
           {
                 max_value =  value_tmp[i];
           }
         if(value_tmp[i] <  min_value)
           {
                 min_value =  value_tmp[i];
           }
        }
   for(i=0;i< number;i++)
    {
         sum += value_tmp[i];
        }
   sample_value = (sum - max_value - min_value)/(number - 2);
   return (sample_value);

}
void gps_enu(void)
{
    //初始化
    if(!init)
    {
        init_pose.latitude = msg_navsatfix.latitude;
        init_pose.longitude = msg_navsatfix.longitude;
        init_pose.altitude = msg_navsatfix.altitude;
        init_status=msg_navsatfix.status.status;
        init = true;

    }
    else
    {
    //计算相对位置
        double radLat1 ,radLat2, radLong1,radLong2,delta_lat,delta_long,x,y;
                radLat1 = rad(init_pose.latitude);
        radLong1 = rad(init_pose.longitude);
                radLat2 = rad(msg_navsatfix.latitude);
                radLong2 = rad(msg_navsatfix.longitude);

        //计算x
        delta_long = 0;
        delta_lat = radLat2 - radLat1;  //(radLat1,radLong1)-(radLat2,radLong1)
        if(delta_lat>0)
        x = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));
        else
        x=-2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));

        x = x*EARTH_RADIUS*1000;

        //计算y
        delta_lat = 0;
        delta_long = radLong2  - radLong1;   //(radLat1,radLong1)-(radLat1,radLong2)
        if(delta_long>0)
        y = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
        else
        y=-2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
        //double y = 2*asin( sin( delta_lat/2 ) + cos( radLat2 )*cos( radLat2)* sin( delta_long/2 )   );
        y = y*EARTH_RADIUS*1000;

        //计算z
        double z = msg_navsatfix.altitude - init_pose.altitude;

        if(enu_use==1) //ENU
              {
               if(v_time>0)
                {
                for(int j=5;j>1;j--)
                       {enu_xv[j]=enu_xv[j-1];enu_yv[j]=enu_yv[j-1];enu_zv[j]=enu_zv[j-1];}
                enu_xv[1]=(y-enu_x)/v_time;
                enu_yv[1]=(x-enu_y)/v_time;
                enu_zv[1]=(z-enu_z)/v_time;
                }
               enu_x=y;
               enu_y=x;
               enu_z=z;
               }
              if(enu_use==2) //NED
              {
               if(v_time>0)
                {
                for(int j=5;j>1;j--)
                       {enu_xv[j]=enu_xv[j-1];enu_yv[j]=enu_yv[j-1];enu_zv[j]=enu_zv[j-1];}
                enu_xv[1]=(x-enu_x)/v_time;
                enu_yv[1]=(y-enu_y)/v_time;
                enu_zv[1]=(-z-enu_z)/v_time;
                }
               enu_x=x;
               enu_y=y;
               enu_z=-z;
               }

              if(v_time>0)
              {
               if(v_time<2)
                {
                   enu_xv[0]=max_min_chioce(&enu_xv[1],5);
                   enu_yv[0]=max_min_chioce(&enu_yv[1],5);
                   enu_zv[0]=max_min_chioce(&enu_zv[1],5);

                }else
                  {
                    enu_xv[0]=enu_xv[1];
                    enu_yv[0]=enu_yv[1];
                    enu_zv[0]=enu_zv[1];
                  }
              }

           }
         //  cout<<"xyzv0 : " <<enu_xv[0]<<","<< enu_yv[0]<<","<< enu_zv[0]<<":" << enu_xv[1]<<","<< enu_yv[1]<<","<< enu_zv[1]<<endl;
        //   cout<<"xyz : " << enu_x<<","<< enu_y<<","<< enu_z<<"," <<endl;
}

void gpggaManager(string &serial_data)
{

        string serialHeader;

        vector<int> separator_pos;
        
        struct SeparatorFormat {
                int first;
                int headerLength ;
                int totalCommas;
        }gpgga,bestxyza,gptra;   //record the locate of every comma;

        gpgga.first = 6;
        gpgga.totalCommas =14;
        bestxyza.first = 9;
        bestxyza.totalCommas = 36; // BESTXYZ NOTICE: 9,s + 1; +  27,s
        gptra.first = 6;
        gptra.totalCommas = 8;

        separator_pos.push_back(serial_data.find(",",0));

        serialHeader.assign(serial_data,1,separator_pos[0]-1);

        vector<string> ggaHeader;

        ggaHeader.push_back("GNGGA");
        ggaHeader.push_back("BESTXYZA");
        ggaHeader.push_back("GNTRA");
        data_c++;
        //cout<<"serial_data size: " << serial_data.size() <<endl;
         dlen= serial_data.size();
        if ((strcmp(serialHeader.c_str(),ggaHeader[0].c_str()) == 0) & (dlen >50)&(serial_data[dlen-1]=='\n')&(ggaflage>0))  //GPGGA
        {
              //   cout << serialHeader.c_str() << endl;
                for(int i=1;i<=gpgga.totalCommas-1;i++)
                {
                        separator_pos.push_back(serial_data.find(",",separator_pos[i-1]+1));
                }

                gpgga_msg.header.stamp = ros::Time::now();
                msg_navsatfix.header.stamp = ros::Time::now();

                v_secs[0] = ros::Time::now().toSec();
                  if(v_secs[1]>0)
                   v_time=v_secs[0]-v_secs[1];

                  v_secs[1]=v_secs[0];
                // cout <<"v_secs"<< setprecision(12)<<v_secs[1] << endl;
                string temp_gpgga;
                temp_gpgga.assign(serial_data,separator_pos[0]+1 ,separator_pos[1]-separator_pos[0]-1);
                gpgga_msg.utc_seconds = stringToNum<double>(temp_gpgga);
                //cout<<"utc_seconds: " << gpgga_msg.utc_seconds <<endl;
                temp_gpgga.assign(serial_data,separator_pos[1]+1 ,separator_pos[2]-separator_pos[1]-1);
                gpgga_msg.lat = stringToNum<double>(temp_gpgga);
                double temp = gpgga_msg.lat/100;
                int integer = floor(temp);
                double decimals =  temp - integer;
               msg_navsatfix.latitude=integer + decimals/0.6;

                gpgga_msg.lat_dir = temp_gpgga.assign(serial_data,separator_pos[2]+1 ,separator_pos[3]-separator_pos[2]-1);

                temp_gpgga.assign(serial_data,separator_pos[3]+1 ,separator_pos[4]-separator_pos[3]-1);
                gpgga_msg.lon = stringToNum<double>(temp_gpgga);
                 temp = gpgga_msg.lon/100;
                 integer = floor(temp);
                 decimals =  temp - integer;
                msg_navsatfix.longitude=integer + decimals/0.6;

                gpgga_msg.lon_dir = temp_gpgga.assign(serial_data,separator_pos[4]+1 ,separator_pos[5]-separator_pos[4]-1);

                temp_gpgga.assign(serial_data,separator_pos[5]+1 ,separator_pos[6]-separator_pos[5]-1);
                gpgga_msg.gps_qual = stringToNum<int>(temp_gpgga);//0初始化， 1单点定位， 2码差分， 3无效PPS， 4固定解， 5浮点解， 6正在估算                
                msg_navsatfix.status.status=gpgga_msg.gps_qual;

                if(status_gps==false)
                 {
                    if( msg_navsatfix.status.status==0)
                    msg_navsatfix.status.status=-1;
                    if( msg_navsatfix.status.status>2)
                    msg_navsatfix.status.status=2;
                 }

                temp_gpgga.assign(serial_data,separator_pos[6]+1 ,separator_pos[7]-separator_pos[6]-1);
                gpgga_msg.num_sats = stringToNum<int>(temp_gpgga);

                temp_gpgga.assign(serial_data,separator_pos[7]+1 ,separator_pos[8]-separator_pos[7]-1);
                gpgga_msg.hdop = stringToNum<double>(temp_gpgga);

                temp_gpgga.assign(serial_data,separator_pos[8]+1 ,separator_pos[9]-separator_pos[3]-1);
                gpgga_msg.alt = stringToNum<double>(temp_gpgga);
                msg_navsatfix.altitude = stringToNum<double>(temp_gpgga);

                gpgga_msg.altitude_units = temp_gpgga.assign(serial_data,separator_pos[9]+1 ,separator_pos[10]-separator_pos[9]-1);

                temp_gpgga.assign(serial_data,separator_pos[10]+1 ,separator_pos[11]-separator_pos[10]-1);//error of horizonal level
                gpgga_msg.undulation = stringToNum<double>(temp_gpgga);

                gpgga_msg.undulation_units = temp_gpgga.assign(serial_data,separator_pos[11]+1 ,separator_pos[12]-separator_pos[11]-1);

                temp_gpgga.assign(serial_data,separator_pos[12]+1 ,separator_pos[13]-separator_pos[12]-1);
                gpgga_msg.diff_age = stringToNum<int>(temp_gpgga);

                gpgga_msg.station_id = temp_gpgga.assign(serial_data,separator_pos[13]+1,4);

        }
        else if ((strcmp(serialHeader.c_str(),ggaHeader[1].c_str()) == 0) & (dlen >120)&(serial_data[dlen-1]=='\n')&(xyzflage>0)) //BESTXYZA
        {
                 //cout << serialHeader.c_str() << endl;

                for(int i=1;i<=bestxyza.totalCommas-1;i++)
                {
                        separator_pos.push_back(serial_data.find(",",separator_pos[i-1]+1));
                }

                int header_separator  = serial_data.find(";",0);

                msg_gnssodometry.header.stamp = ros::Time::now();
                msg_gnssodometry.header.frame_id = "gnss";

                string temp_bestxyza;
                temp_bestxyza.assign(serial_data,header_separator+1 ,separator_pos[9]-header_separator-1);  //x in ECEF

                if(strcmp(temp_bestxyza.c_str(),"SOL_COMPUTED") == 0) //已解出，数据有效
                {


                }else
                {
                        //msg_navsatfix.status.status = -1;

                }


                temp_bestxyza.assign(serial_data,separator_pos[10]+1 ,separator_pos[11]-separator_pos[10]-1);  //x in ECEF
            msg_gnssodometry.pose.pose.position.x = stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[11]+1 ,separator_pos[12]-separator_pos[11]-1);  //y in ECEF
            msg_gnssodometry.pose.pose.position.y = stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[12]+1 ,separator_pos[13]-separator_pos[12]-1);  //z in ECEF
            msg_gnssodometry.pose.pose.position.z = stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[13]+1 ,separator_pos[14]-separator_pos[13]-1);  //std variance x in ECEF
            msg_gnssodometry.pose.covariance[0] = stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza) ;
                msg_navsatfix.position_covariance[0] = stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);


                temp_bestxyza.assign(serial_data,separator_pos[14]+1 ,separator_pos[15]-separator_pos[14]-1);  //std variance y in ECEF
            msg_gnssodometry.pose.covariance[7] = stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);
            msg_navsatfix.position_covariance[4] = stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[15]+1 ,separator_pos[16]-separator_pos[15]-1);  //std variance z in ECEF
            msg_gnssodometry.pose.covariance[14] = stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);
            msg_navsatfix.position_covariance[8] = stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[18]+1 ,separator_pos[19]-separator_pos[18]-1);  //vx in ECEF
                msg_gnssodometry.twist.twist.linear.x = stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[19]+1 ,separator_pos[20]-separator_pos[19]-1);  //vy in ECEF
                msg_gnssodometry.twist.twist.linear.y = stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[20]+1 ,separator_pos[21]-separator_pos[20]-1);  //vz in ECEF
                msg_gnssodometry.twist.twist.linear.z = stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[21]+1 ,separator_pos[22]-separator_pos[21]-1);  //std variance vx in ECEF
                msg_gnssodometry.twist.covariance[0]= stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[22]+1 ,separator_pos[23]-separator_pos[22]-1);  //std variance vy in ECEF
                msg_gnssodometry.twist.covariance[7]= stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);

                temp_bestxyza.assign(serial_data,separator_pos[23]+1 ,separator_pos[24]-separator_pos[23]-1);  //std variance vz in ECEF
                msg_gnssodometry.twist.covariance[14]= stringToNum<double>(temp_bestxyza) * stringToNum<double>(temp_bestxyza);

            msg_navsatfix.position_covariance_type = 3;

        }
        else if ((strcmp(serialHeader.c_str(),ggaHeader[2].c_str()) == 0) & (dlen >30)&(serial_data[dlen-1]=='\n')&(traflage>0))   //GPTRA
        {

               for(int i=1;i<=gptra.totalCommas-1;i++)
                {
                        separator_pos.push_back(serial_data.find(",",separator_pos[i-1]+1));
                }
                msg_gnssodometry.header.stamp = ros::Time::now();
                msg_gnssodometry.header.frame_id = "gnss";

                string temp_gptra;
                temp_gptra.assign(serial_data,separator_pos[1]+1 ,separator_pos[2]-separator_pos[1]-1);

                z_yaw= stringToNum<double>(temp_gptra);

               if(enu_use==1) //ENU
                t_yaw=90-z_yaw;
               else t_yaw=z_yaw;

               if(t_yaw<0)t_yaw=t_yaw+360;

              gnss_heading = t_yaw / 180 * PI;


          temp_gptra.assign(serial_data,separator_pos[2]+1 ,separator_pos[3]-separator_pos[2]-1);
           gnss_pitch= stringToNum<double>(temp_gptra) / 180 * PI;
             if(enu_use==1) //ENU
               gnss_pitch=-gnss_pitch;
          temp_gptra.assign(serial_data,separator_pos[3]+1 ,separator_pos[4]-separator_pos[3]-1);
           gnss_roll= stringToNum<double>(temp_gptra) / 180 * PI;

          Eigen::Vector3d ea0(gnss_heading,gnss_pitch,gnss_roll);
      Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())
        * Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());

        Eigen::Quaterniond q;
        q = R;
     // cout << q.x() << endl << endl;
     //  cout << q.y() << endl << endl;
     // cout << q.z() << endl << endl;
     // cout << q.w() << endl << endl;

      msg_gnssodometry.pose.pose.orientation.x = q.x();
      msg_gnssodometry.pose.pose.orientation.y = q.y();
      msg_gnssodometry.pose.pose.orientation.z = q.z();
      msg_gnssodometry.pose.pose.orientation.w = q.w();
                // Eigen::Matrix3d Rx = q.toRotationMatrix();
         //    Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);
  //   	cout << ea1/PI*180 - ea0 << endl << endl;

        }
   if(data_c>59)
      {   
       data_c=0;
  if((msg_navsatfix.status.status>0)&((msg_navsatfix.latitude!=0)|(msg_navsatfix.longitude!=0)))
{out=5;cout<<msg_navsatfix.header.stamp.sec<<" POS: " <<gpgga_msg.gps_qual<<","
 <<fixed<<setprecision(8)<<msg_navsatfix.latitude<<"," << msg_navsatfix.longitude 
<<","<<fixed<<setprecision(2) <<msg_navsatfix.altitude
<<"m Pih:"<< gnss_pitch*180/PI <<"° "<<" Rol:"<< gnss_roll*180/PI <<"° "<<" Yaw:"<< z_yaw  <<"°"<<endl;
      cout<<msg_navsatfix.header.stamp.sec<<" "<<enu_s<<": " <<init_status<<","<<fixed<<setprecision(8)<<init_pose.latitude<<","<< init_pose.longitude
      <<","<<fixed<<setprecision(2) <<init_pose.altitude<<"m"<<"  X:"<<enu_x<<" Y:"<<enu_y<<" Z:"<<enu_z<<" Yaw:"<<gnss_heading*180/PI<<endl;
  }
  else {if(out>0)out--;else cout<<msg_navsatfix.header.stamp.sec<<" No satellite signal, please use outdoors"<<endl;}
       }
        return;
}

// void writeCallback(const std_msgs::String::ConstPtr& msg)
// {
//     ROS_INFO_STREAM("Writing to serial port  " <<msg->data);

//     ser.write(msg->data+ "\r\n"); //发送串口数据

//     cout << msg->data << endl;
// }


int main (int argc, char** argv) {

        ros::init(argc, argv, "serial_node");

        ros::NodeHandle nh;
        ros::NodeHandle param_nh("~");
        string port,vst,ggas,tras,xyzs;
        int Baudrate,time_out,ggaflag=0,traflag=0,xyzflag=0,ggaone=0;
        float gpgga_freq,gptra_freq,bestxyza_freq;
        bool gpgga_enable,gptra_enable,bestxyza_enable,Unlogall_enable;

        param_nh.param("gnss_port",port,string("/dev/ttyUSB0"));
        param_nh.param("gnss_Baudrate",Baudrate,115200);
        param_nh.param("serial_timeout",time_out,5);
        param_nh.param("enu_use",enu_use,0);
        if(enu_use==0)enu_s="ECEF";
        if(enu_use==1)enu_s="ENU";
        if(enu_use==2)enu_s="NED";

       // nh.param("serial_timeout",time_out,5);
        nh.param("gpgga_enable",gpgga_enable,false);
        nh.param("gptra_enable",gptra_enable,false);
        nh.param("bestxyza_enable",bestxyza_enable,false);
        nh.param("Unlogall_enable",Unlogall_enable,false);
        nh.param<float>("gpgga_freq",gpgga_freq,1);
        nh.param<float>("gptra_freq",gptra_freq,1);
        nh.param<float>("bestxyza_freq",bestxyza_freq,1);


        // ros::Subscriber write_sub = nh.subscribe("writeToSerial", 1, writeCallback);


        ros::Publisher read_pub = nh.advertise<nmea_msgs::Gpgga>("gpgga", 1);
        ros::Publisher read_pub2 = nh.advertise<nav_msgs::Odometry>("navOdometry", 1);
        ros::Publisher read_pub3 = nh.advertise<sensor_msgs::NavSatFix>("navSatFix", 1);
        cout <<"use port:"<<port<< "  Baudrate:"<<Baudrate<< endl;
        //设置串口属性，并打开串口
        ser.setPort(port);
        ser.setBaudrate(Baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(time_out);
        ser.setTimeout(to);
        try
        {
                ser.open();
                }
                catch (serial::IOException& e)
                {
                ROS_ERROR_STREAM("Unable to open port ");
                return -1;
        }

        if(ser.isOpen())
        {

                ROS_INFO_STREAM("Serial Port initialized");
                }
                else
                {
                 return -1;
        }

        if(gpgga_enable)
        {
                cout << "GPGGA enable" << endl;
            ser.write("log com1 gpgga ontime "+numToString(gpgga_freq)+"\r\n"); //发送串口数据
                        cout << "log com1 gpgga ontime "+numToString(gpgga_freq)+"\r\n" <<endl;
        }

        if(gptra_enable)
        {
                cout << "GPTRA enable" << endl;
            ser.write("log com1 gptra onchanged\r\n"); //发送串口数据
        }
        if(bestxyza_enable)
        {
                cout << "BESTXYZA enable" << endl;
            ser.write("log com1 bestxyza ontime "+numToString(bestxyza_freq)+"\r\n"); //发送串口数据
        }
        if(Unlogall_enable)
        {

            ser.write("Unlogall\r\n"); //发送串口数据
        }

        // 50hz频率执行
        ros::Rate loop_rate(50); //set the interval time for publishing message, if not set the rate, ros will try to publish as much as possible.
        ser.flush();//等待串口数据发送结束

        while(ros::ok())
        {
                if(ser.available())
                {

                        // ROS_INFO_STREAM("Reading from serial port\n");
                        int jj=0;
                        vector<std::string> vs;
                        nmea_msgs::Gpgga msg_gpgga;


                        // cout<<"avali before read: "<<ser.available()<<endl;
                        vs = ser.readlines(); //是读一行,以\n结束

        //		 cout<<"avali after read: "<<ser.available()<<endl;
        //		 cout<<"read size: " << vs.size() <<endl;


                        for(int i = 0 ; i<vs.size(); i++)
                        {
                              //cout<<"read contents: " << vs[i] <<endl;
                              vst=vst+vs[i];

                        }

                        dlen=vst.size();
                        if(vst[dlen-1]=='\n')
                          {
                          // cout<<"dlen: " << dlen <<endl;
                          // cout<<"vst: " << vst <<endl;

                              for(int j = 0 ; j<dlen; j++)
                             {

                                  if(ggaflag>0 && ggaflage==0 && jj>0)
                                  {
                                      ggas=ggas+vst[j];
                                      jj++;
                                    if(vst[j]=='\n') { ggaflage=1;ggaflag=0;jj=0; }

                                  }

                               if(vst[j-2]=='G'&& vst[j-1]=='G'&& vst[j]=='A'&& ggaflag==0 &&  j>4 && jj==0)
                               {
                                   if(ggaone==2)
                                   { ROS_INFO_STREAM("GGA DATA OK");
                                       if(enu_use==1)
                                        ROS_INFO_STREAM("ENU XYZ");
                                       if(enu_use==2)
                                        ROS_INFO_STREAM("NED XYZ");
                                       if(enu_use==0)
                                         ROS_INFO_STREAM("ECEF XYZ");

                                      ggaone=3;
                                   }

                                 ggaflag=1;
                                 ggaflage=0;
                                 ggas=vst[j-5];
                                 ggas=ggas+vst[j-4]+vst[j-3]+vst[j-2]+vst[j-1]+vst[j];
                                 jj=6;
                                 //cout<<"GGA0: " << ggas <<endl;
                               }
                            // ///////////////
                               if(xyzflag>0 && xyzflage==0 && jj>0)
                               {
                                   
                                   xyzs=xyzs+vst[j];
                                   jj++;
                                 if(vst[j]=='\n') { xyzflage=1;xyzflag=0;jj=0; }

                               }

                            if(vst[j-2]=='S'&& vst[j-1]=='T'&& vst[j]=='X'&& xyzflag==0 &&  j>4 && jj==0)
                            {
                              if(ggaone==1)ggaone=2;  
                              xyzflag=1;
                              xyzflage=0;
                              xyzs=vst[j-5];
                              xyzs=xyzs+vst[j-4]+vst[j-3]+vst[j-2]+vst[j-1]+vst[j];
                              jj=6;

                            }
                             // //////////////
                            if(traflag>0 && traflage==0 && jj>0)
                            {
                                
                                tras=tras+vst[j];
                                jj++;
                              if(vst[j]=='\n') { traflage=1;traflag=0;jj=0; }

                            }

                         if(vst[j-2]=='T'&& vst[j-1]=='R'&& vst[j]=='A'&& traflag==0 &&  j>4 && jj==0)
                         {
                           if(ggaone==0)ggaone=1;  
                           traflag=1;
                           traflage=0;
                           tras=vst[j-5];
                           tras=tras+vst[j-4]+vst[j-3]+vst[j-2]+vst[j-1]+vst[j];
                           jj=6;

                         }



                             }

                           if(ggaflage>0)
                           {
                             //  cout<<"GGA: " << ggas <<endl;
                           gpggaManager(ggas);
                           //if((msg_navsatfix.status.status>0)&((msg_navsatfix.latitude!=0)|(msg_navsatfix.longitude!=0)))
                            {
                            read_pub.publish(msg_gpgga);
                            read_pub3.publish(msg_navsatfix);
                            if(msg_navsatfix.status.status>0)
                                 gps_enu();
                            }
                            ggaflage=0;
                           }
                           if(traflage+xyzflage>0 )
                           {
                             //  cout<<"XYZ: " << xyzs <<endl;
                            if(traflage>0)
                            gpggaManager(tras);
                            if(xyzflage>0)
                            gpggaManager(xyzs);

                            if(enu_use>0&&init)
                             {
                               msg_gnssodometry.pose.pose.position.x=enu_x;
                               msg_gnssodometry.pose.pose.position.y=enu_y;
                               msg_gnssodometry.pose.pose.position.z=enu_z;
                               msg_gnssodometry.twist.twist.linear.x=enu_xv[0];
                               msg_gnssodometry.twist.twist.linear.y=enu_yv[0];
                               msg_gnssodometry.twist.twist.linear.z=enu_zv[0];
                             }

                            read_pub2.publish(msg_gnssodometry);

                           if(xyzflage>0)xyzflage=0;
                           if(traflage>0)traflage=0;
                           }
                           vst="";
                           }




                        ser.flush();

                }


        // 处理ROS的信息，比如订阅消息,并调用回调函数
        ros::spinOnce();

        loop_rate.sleep();

        }


}

